﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Emgu.CV;
using Emgu.CV.UI;
using Emgu.CV.Structure;

namespace VisionTestApp2
{
    /// <summary>
    /// Lucas-Kanade method (based on code from http://www.codeproject.com/Articles/24809/Image-Alignment-Algorithms)
    /// </summary>
    public static class ForwardsAdditive
    {
        public static EstimatedPose FindPose(Image<Gray, Byte> source, Image<Gray, Byte> template, EstimatedPose startingPose = null)
        {
            // Some constants for iterative minimization process.
	        const float EPS = 1E-5f; // Threshold value for termination criteria.
	        const int MAX_ITER = 100;  // Maximum iteration count.
	
	        // Here we will store matrices.
	        Matrix<float> W = new Matrix<float>(3,3);  // Current value of warp W(x,p)
	        Matrix<float> X = new Matrix<float>(3,1);  // Point in coordinate frame of T.
	        Matrix<float> Z = new Matrix<float>(3,1);  // Point in coordinate frame of I.
	
	        Matrix<float> H = new Matrix<float>(3,3);  // Hessian
	        Matrix<float> iH = new Matrix<float>(3,3); // Inverse of Hessian
	        Matrix<float> b = new Matrix<float>(3,1);  // Vector in the right side of the system of linear equations.
	        Matrix<float> delta_p = new Matrix<float>(3,1); // Parameter update value.
	
	        /*
	         *  Precomputation stage.
	         */

	        // Calculate gradient of I.
            Image<Gray,float> pGradIx = source.Sobel(1,0,3); // Gradient in X direction (TS: is aperture size right?)
	        pGradIx = pGradIx.ConvertScale<float>(0.125,0); // Normalize
	        
	        Image<Gray,float> pGradIy = source.Sobel(0,1,3); // Gradient in Y direction (TS: is aperture size right?)
	        pGradIy = pGradIy.ConvertScale<float>(0.125,0); // Normalize

	        /*
	         *  Iteration stage.
	         */

	        // Here we will store parameter approximation. 
	        float wz_a=0, tx_a=0, ty_a=0;

            if (startingPose != null)
            {
                tx_a = startingPose.X;
                ty_a = startingPose.Y;
                wz_a = startingPose.Theta;
            }
	        // Here we will store current mean error value.
	        float mean_error=0;

	        // Iterate
	        int iter=0; // number of current iteration
	        while(iter < MAX_ITER)
	        {
		        iter++; // Increment iteration counter

		        mean_error = 0; // Set mean error value with zero

		        int pixel_count=0; // Count of processed pixels
		
		        W = InitWarp(wz_a, tx_a, ty_a); // Init warp W(x, p)
		        H.SetIdentity(); // Set Hessian with zeroes
		        b.SetIdentity(); // Set b matrix with zeroes

		
		        // (u,v) - pixel coordinates in the coordinate frame of T.
		        int u, v;

		        // Walk through pixels in the template T.
		        int i, j;
		        for(i=0; i<template.Width; i++)
		        {
			        u = i;

			        for(j=0; j<template.Height; j++)
			        {
				        v = j;

				        // Set vector X with pixel coordinates (u,v,1)
                        X[0,0] = u;
                        X[1,0] = v;
                        X[2,0] = 1;

				        // Warp Z=W*X
                        Z = W.Mul(X);
				        //cvGEMM(W, X, 1, 0, 0, Z); (TS: is this right?)

				        // (u2,v2) - pixel coordinates in the coordinate frame of I.
				        // Get coordinates of warped pixel in coordinate frame of I.
                        float u2 = Z[0,0];
                        float v2 = Z[1,0];

				        // Get the nearest integer pixel coords (u2i;v2i).
				        int u2i = (int)Math.Floor(u2);
				        int v2i = (int)Math.Floor(v2);

				        if(u2i>=0 && u2i<source.Width && // check if pixel is inside I.
					        v2i>=0 && v2i<source.Height)
				        {
					        pixel_count++;

					        // Evaluate gradient of I at W(x,p) with subpixel accuracy
					        // using bilinear interpolation.
					        float Ix = interpolate<short>(pGradIx, u2, v2);
					        float Iy = interpolate<short>(pGradIy, u2, v2);

					        // Calculate steepest descent image's element.
					        float[] stdesc = new float[3]; // an element of steepest descent image
					        stdesc[0] = (float)(-v*Ix+u*Iy);
					        stdesc[1] = (float)Ix;
					        stdesc[2] = (float)Iy;

					        // Calculate intensity of a transformed pixel with sub-pixel accuracy
					        // using bilinear interpolation.
					        float I2 = interpolateByte<short>(source, u2, v2);
					
					        // Calculate image difference D = T(x)-I(W(x,p)).
					        float D = (float)template[v, u].Intensity - I2;
					
					        // Update mean error value.
					        mean_error += Math.Abs(D);

					        // Add a term to b matrix.
					        b[0,0] += stdesc[0] * D;
					        b[1,0] += stdesc[1] * D;
					        b[2,0] += stdesc[2] * D;
					
					        // Add a term to Hessian.
					        int l,m;
					        for(l=0;l<3;l++)
					        {
						        for(m=0;m<3;m++)
						        {
                                    H[l,m] += stdesc[l]*stdesc[m];
						        }
					        }
				        }	
			        }
		        }

		        // Finally, calculate mean error.
		        if(pixel_count!=0)
			        mean_error /= pixel_count;

		        // Invert Hessian.
		        //double inv_res = cvInvert(H, iH);
                double inv_res = CvInvoke.cvInvert(H.Ptr, iH.Ptr, Emgu.CV.CvEnum.SOLVE_METHOD.CV_SVD_SYM); //TS: is solve method right?
                

                if(inv_res==0)
		        {
			       // printf("Error: Hessian is singular.\n");
			        return new EstimatedPose();
		        }

		        // Find parameter increment. 
		        //cvGEMM(iH, b, 1, 0, 0, delta_p);
                delta_p = iH.Mul(b);
		        
                float delta_wz = delta_p[0,0];
		        float delta_tx = delta_p[1,0];
		        float delta_ty = delta_p[2,0];

		        // Update parameter vector approximation.
		        wz_a += delta_wz;
		        tx_a += delta_tx;
		        ty_a += delta_ty;

		        // Print diagnostic information to screen.
		       // printf("iter=%d dwz=%f dtx=%f dty=%f mean_error=%f\n", 
			     //   iter, delta_wz, delta_tx, delta_ty, mean_error);

		        // Check termination critera.
		        if(Math.Abs(delta_wz)<EPS && Math.Abs(delta_tx)<EPS && Math.Abs(delta_ty)<EPS) break;
	        }

	        
	        // Print summary.
            //printf("===============================================\n");
            //printf("Algorithm: forward additive.\n");
            //printf("Caclulation time: %g sec.\n", total_time);
            //printf("Iteration count: %d\n", iter);
            //printf("Approximation: wz_a=%f tx_a=%f ty_a=%f\n", wz_a, tx_a, ty_a);
            //printf("Epsilon: %f\n", EPS);
            //printf("Resulting mean error: %f\n", mean_error);
            //printf("===============================================\n");

	        // Show result of image alignment.
	        W = InitWarp(wz_a, tx_a, ty_a);
	        //DrawWarpedRectangle(pImgI, omega, W);
	        //cvSetImageROI(pImgT, omega);
	        //cvShowImage("template",pImgT);
	        //cvShowImage("image",pImgI);
	        //cvResetImageROI(pImgT);

            //// Free used resources and exit.
            //cvReleaseMat(&W);
            //cvReleaseMat(&X);
            //cvReleaseMat(&Z);
	
            //cvReleaseMat(&H);
            //cvReleaseMat(&iH);
            //cvReleaseMat(&b);
            //cvReleaseMat(&delta_p);

            return new EstimatedPose(){ Theta=wz_a, X=tx_a, Y=ty_a, Score=mean_error};
        }


        private static Matrix<float> InitWarp(float wz, float tx, float ty)
        {
            Matrix<float> W = new Matrix<float>(3, 3);

            W[0, 0] = 1;
            W[1, 0] = wz;
            W[2, 0] = 0;
	        //CV_MAT_ELEM(*W, float, 0, 0) = 1;
	        //CV_MAT_ELEM(*W, float, 1, 0) = wz;
	        //CV_MAT_ELEM(*W, float, 2, 0) = 0;

            W[0, 1] = -wz;
            W[1, 1] = 1;
            W[2, 1] = 0;
	        //CV_MAT_ELEM(*W, float, 0, 1) = -wz;
	        //CV_MAT_ELEM(*W, float, 1, 1) = 1;
	        //CV_MAT_ELEM(*W, float, 2, 1) = 0;

            W[0, 2] = tx;
            W[1, 2] = ty;
            W[2, 2] = 1;
	        //CV_MAT_ELEM(*W, float, 0, 2) = tx;
	        //CV_MAT_ELEM(*W, float, 1, 2) = ty;
	        //CV_MAT_ELEM(*W, float, 2, 2) = 1;

            return W;
        }

        //private void DrawWarpedRectangle(IplImage* pImage, CvRect rect, CvMat* W)
        //{
        //    CvPoint lt, lb, rt, rb;

        //    CvMat* X = cvCreateMat(3, 1, CV_32F);
        //    CvMat* Z = cvCreateMat(3, 1, CV_32F);

        //    // left-top point
        //    SET_VECTOR(X, rect.x, rect.y);
        //    cvGEMM(W, X, 1, 0, 0, Z);
        //    GET_INT_VECTOR(Z, lt.x, lt.y);

        //    // left-bottom point
        //    SET_VECTOR(X, rect.x, rect.y + rect.height);
        //    cvGEMM(W, X, 1, 0, 0, Z);
        //    GET_INT_VECTOR(Z, lb.x, lb.y);

        //    // right-top point
        //    SET_VECTOR(X, rect.x + rect.width, rect.y);
        //    cvGEMM(W, X, 1, 0, 0, Z);
        //    GET_INT_VECTOR(Z, rt.x, rt.y);

        //    // right-bottom point
        //    SET_VECTOR(X, rect.x + rect.width, rect.y + rect.height);
        //    cvGEMM(W, X, 1, 0, 0, Z);
        //    GET_INT_VECTOR(Z, rb.x, rb.y);

        //    // draw rectangle
        //    cvLine(pImage, lt, rt, cvScalar(255));
        //    cvLine(pImage, rt, rb, cvScalar(255));
        //    cvLine(pImage, rb, lb, cvScalar(255));
        //    cvLine(pImage, lb, lt, cvScalar(255));

        //    // release resources and exit
        //    cvReleaseMat(&X);
        //    cvReleaseMat(&Z);
        //}

        //private static void WarpImage(IplImage* pSrcFrame, IplImage* pDstFrame, CvMat* W)
        //{
        //    cvSet(pDstFrame, cvScalar(0));

        //    Matrix<float> X = new Matrix<float>(3, 1);
        //    Matrix<float> Z = new Matrix<float>(3, 1);

        //    for (int x = 0; x < pSrcFrame->width; x++)
        //    {
        //        for (int y = 0; y < pSrcFrame->height; y++)
        //        {
        //            SET_VECTOR(X, x, y);

        //            cvGEMM(W, X, 1, 0, 0, Z);

        //            int x2, y2;
        //            GET_INT_VECTOR(Z, x2, y2);

        //            if (x2 >= 0 && x2 < pDstFrame->width &&
        //                y2 >= 0 && y2 < pDstFrame->height)
        //            {
        //                CV_IMAGE_ELEM(pDstFrame, uchar, y2, x2) =
        //                    CV_IMAGE_ELEM(pSrcFrame, uchar, y, x);
        //            }
        //        }
        //    }

        //    //cvSmooth(pDstFrame, pDstFrame);
        //}

        // Interpolates pixel intensity with subpixel accuracy.
        // Abount bilinear interpolation in Wikipedia:
        // http://en.wikipedia.org/wiki/Bilinear_interpolation
        private static float interpolate<T>(Image<Gray,float> image, float x, float y)
        {
	        // Get the nearest integer pixel coords (xi;yi).
	        int xi = (int) Math.Floor(x);
	        int yi = (int) Math.Floor(y);

	        float k1 = x-xi; // Coefficients for interpolation formula.
	        float k2 = y-yi;

	        bool f1 = xi<image.Width-1;  // Check that pixels to the right  
	        bool f2 = yi<image.Height-1; // and to down direction exist.

            double row1a = image[yi, xi].Intensity;
            double row1b = image[yi, xi+1].Intensity;
            double row2a = image[yi + 1, xi].Intensity;
            double row2b = image[yi + 1, xi+1].Intensity;
            //T* row1 = &CV_IMAGE_ELEM(pImage, T, yi, xi);
            //T* row2 = &CV_IMAGE_ELEM(pImage, T, yi+1, xi);
				
	        // Interpolate pixel intensity.
	        float interpolated_value = (1.0f-k1)*(1.0f-k2)*(float)row1a +
				        (f1 ? ( k1*(1.0f-k2)*(float)row1b ):0) +
				        (f2 ? ( (1.0f-k1)*k2*(float)row2a ):0) +						
				        ((f1 && f2) ? ( k1*k2*(float)row2b ):0) ;

	        return interpolated_value;

        }

        // Interpolates pixel intensity with subpixel accuracy.
        // Abount bilinear interpolation in Wikipedia:
        // http://en.wikipedia.org/wiki/Bilinear_interpolation
        private static float interpolateByte<T>(Image<Gray, Byte> image, float x, float y)
        {
            // Get the nearest integer pixel coords (xi;yi).
            int xi = (int)Math.Floor(x);
            int yi = (int)Math.Floor(y);

            float k1 = x - xi; // Coefficients for interpolation formula.
            float k2 = y - yi;

            bool f1 = xi < image.Width - 1;  // Check that pixels to the right  
            bool f2 = yi < image.Height - 1; // and to down direction exist.

            double row1a = image[yi, xi].Intensity;
            double row1b = image[yi, xi + 1].Intensity;
            double row2a = image[yi + 1, xi].Intensity;
            double row2b = image[yi + 1, xi + 1].Intensity;
            //T* row1 = &CV_IMAGE_ELEM(pImage, T, yi, xi);
            //T* row2 = &CV_IMAGE_ELEM(pImage, T, yi+1, xi);

            // Interpolate pixel intensity.
            float interpolated_value = (1.0f - k1) * (1.0f - k2) * (float)row1a +
                        (f1 ? (k1 * (1.0f - k2) * (float)row1b) : 0) +
                        (f2 ? ((1.0f - k1) * k2 * (float)row2a) : 0) +
                        ((f1 && f2) ? (k1 * k2 * (float)row2b) : 0);

            return interpolated_value;

        }
    }
}
